#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Hubs,  S2, HTServo,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     Arm1,          tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     Arm2,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     LeftMid,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     LeftSides,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     RightMid,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     RightSides,    tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C1_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    goalGrabber,          tServoStandard)
#pragma config(Servo,  srvo_S2_C2_1,    servo7,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_2,    servo8,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_3,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    ballCollecter,        tServoStandard)
#pragma config(Servo,  srvo_S2_C2_6,    wrist,                tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

 #define TANK_4M
#include "../library/drive_modes/drive_modes.h"
#include "../library/controllers/tank_controller.c"
#include "includes/init_teleop.c"

#pragma debuggerWindows("nxtLCDScreen");
//#pragma debuggerWindows("Servos");
#pragma debuggerWindows("joystickSimple");

#include "JoystickDriver.c"

/*void servoPosition()
{
	int servoMultiplier = nMotorEncoder[Arm] / ServoValue[wrist];

	servo[wrist] = ServoValue[wrist] * servoMultiplier;
}*/

task main()
{
  RegisterDriveMotors(LeftMid, LeftSides, RightMid, RightSides);
	InitializeTeleOP();

	waitForStart();

	StartTask(DriveTank);
	while(true)
	{
		if (joy2Btn(3) == 1)
		{
			servo[goalGrabber] = 150;
		}
		else if (joy2Btn(2) == 1)
		{
			servo[goalGrabber] = 45;
		}


		//Andymark motor on the right side of the robot has a 512 ticks per rotation value.
		//split the motors
		//arm raiser
		if (joy2Btn(8) == 1)
		{
			motor[Arm1] = 70;
			motor[Arm2] = 70;
		}
		else if (joy2Btn(7) == 1)
		{
			motor[Arm1] = 40;
			motor[Arm2] = -40;
		}
		else
		{
			motor[Arm1] = 0;
			motor[Arm2] = 0;
		}

		if (abs(joystick.joy2_y2) > 15)
		{
			servo[wrist] = joystick.joy2_y2;
		}

		//arm extender
		/*if (joy2Btn(8) == 1)
		{
			servo[leftExtender] = 255;
			servo[rightExtender] = 0;
		}
		else if (joy2Btn(7) == 1)
		{
			servo[leftExtender] = 0;
			servo[rightExtender] = 255;
		}
		else
		{
			servo[leftExtender] = 128;
			servo[rightExtender] = 128;
		}*/

		//tube code
		if (joy2Btn(4) == 1)
		{
			servo[ballCollecter] = 150;
		}
		else if (joy2Btn(1) == 1)
		{
			servo[ballCollecter] = 45;
		}


		/*while (joy2Btn(7) == 1 && nMotorEncoder[Arm] <= 1110)
		{
			motor[Arm] = 50;
		}
		nMotorEncoder[Arm] = 0;

		while(joy2Btn(6) == 1 && nMotorEncoder[Arm] >= -1110)
		{
			motor[Arm] = -50;

		}
		nMotorEncoder[Arm] = 0;*/
	}
}
